#include "stm32f10x.h"                  // Device header
#include "Delay.h" 
#include "OLED.h"
#include "Encoder.h"
#include "PWM.h"
#include "Servo.h"
#include "string.h"
#include "Serial.h"

extern int16_t Encoder_Count1,Encoder_Count2,Encoder_Count3,Encoder_Count4;
extern int16_t Serial_Count1,Serial_Count2,Serial_Count3,Serial_Count4;

 
int main(void)
{
	
	OLED_Init();		
	PWM_Init();
   Encoder_Init();
	Serial_Init();
	while (1)
	{
		OLED_ShowSignedNum(1, 5, Encoder_Count1, 5);	
		OLED_ShowSignedNum(2, 5, Encoder_Count2, 5);	
		OLED_ShowSignedNum(3, 5, Encoder_Count3, 5);	
		OLED_ShowSignedNum(4, 5, Encoder_Count4, 5);	
		Delay_ms(50);
		
		if(Encoder_Count1>180)         {Encoder_Count1=180;}	
		else if(Encoder_Count1<0)      {Encoder_Count1=0;}
		else if(Encoder_Count2>180)    {Encoder_Count2=180;}	
		else if(Encoder_Count2<0)      {Encoder_Count2=0;}	
		else if(Encoder_Count3>180)    {Encoder_Count3=180;}	
		else if(Encoder_Count3<0)      {Encoder_Count3=0;}	
		else if(Encoder_Count4>35)     {Encoder_Count4=35;}	
		else if(Encoder_Count4<0)      {Encoder_Count4=0;}
			
		else if (Serial_RxFlag == 1)
		  {
			if(strcmp(Serial_RxPacket, "1") == 0)               {Encoder_Count1 += 2;}
         else if (strcmp(Serial_RxPacket, "-1") == 0)        {Encoder_Count1 -= 2;}
			else if (strcmp(Serial_RxPacket, "2") == 0)         {Encoder_Count2 += 2;}
         else if (strcmp(Serial_RxPacket, "-2") == 0)        {Encoder_Count2 -= 2;}
         else if (strcmp(Serial_RxPacket, "3") == 0)         {Encoder_Count3 += 2;}
         else if (strcmp(Serial_RxPacket, "-3") == 0)        {Encoder_Count3 -= 2;}
         else if (strcmp(Serial_RxPacket, "4") == 0)         {Encoder_Count4 += 2;}
         else if (strcmp(Serial_RxPacket, "-4") == 0)        {Encoder_Count4 -= 2;}	
			else
			{
			   OLED_ShowString(1, 1, "                ");
				OLED_ShowString(2, 1, "                ");
				OLED_ShowString(3, 1, "                ");
				OLED_ShowString(4, 1, "                ");
				Serial_SendString("ERROR_COMMAND\r\n");
				OLED_ShowString(2, 1, "                ");
				OLED_ShowString(2, 1, "ERROR_COMMAND");
			}
			
			Serial_RxFlag = 0;
		}
			
		Servo_SetAngle1(Encoder_Count1);
		Servo_SetAngle2(Encoder_Count2);
		Servo_SetAngle3(Encoder_Count3);
		Servo_SetAngle4(Encoder_Count4);	
			
	}
}

